//
// Created by az on 2020/6/20.
//
#include <eigen3/Eigen/Dense>
#include <list>
#include <memory>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include "wmr_msgs/PolynomialTrajectory.h"

#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

//#include "opencv2/core.hpp"
//#include "opencv2/highgui.hpp"

#include "map_generator.h"
#include "corridor_generator.h"
#include "data_type.h"
#include "bezier_base.h"
#include "trajectory_generator.h"
#include "oriTrans.h"

// simulation param from launch file
double resolution_, inv_resolution_, map_margin_, inflate_, cube_margin_;
double check_horizon_, stop_horizon_;
double x_size_, y_size_, z_size_, x_local_size_, y_local_size_, z_local_size_;
double MAX_Vel_, MAX_Acc_;
double vis_traj_width_;
bool   is_limit_vel_, is_limit_acc_, is_proj_cube_, is_retraj_prog_;
int    step_length_, max_inflate_iter_, traj_order_;
double minimize_order_;

bool has_map_   = false;
bool has_target_= false;
bool has_pos_   = false;
bool has_traj_  = false;
bool has_pcd_   = false;  //判断是否有点云
bool is_emerg_  = false;
bool is_AUTO_   = false;
ros::Time start_time_ = ros::TIME_MAX, time_now_ = ros::TIME_MAX;

mutex position_lock_;
Eigen::Vector3d start_pt_, start_vel_, start_acc_, end_pt_, traj_start_pt_;
Eigen::Vector3d last_vel_ = Eigen::Vector3d::Zero();
Eigen::Quaterniond start_quan_;
double robot_vel_, robot_avl_, robot_yaw_;

//map
std::shared_ptr<MAP_GENERATOR> map_generator_;
std::shared_ptr<sdf_tools::CollisionMapGrid> global_map_;
std::shared_ptr<sdf_tools::CollisionMapGrid> local_map_;
//corridor
std::shared_ptr<CORRIDOR_GENERATOR> corridor_2d_;
// bezier basis constant
Eigen::MatrixXd MQM_, FM_;
Eigen::VectorXd C_, Cv_, Ca_, Cj_;
// trajectory related
int seg_num_;
Eigen::VectorXd seg_time_;
Eigen::MatrixXd bezier_coeff_;
// trajectory generate
TrajectoryGenerator trajectory_generator_;
wmr_msgs::PolynomialTrajectory traj_;
int traj_id_ = 1;

ros::Publisher vis_map_pub_, vis_path_pub_, vis_corridor_pub_, vis_traj_pub_, traj_pub_, ESDF_map_pub_, vis_rpos_pub_;
ros::Publisher checkTraj_vis_pub_, stopTraj_vis_pub_;
ros::Publisher is_Auto_pub_;
visualization_msgs::Marker map_marker_;
void visRobotPos(Eigen::Vector3d pos, double yaw);
void visMapPub(const std::string& _name);
void visPath(std::vector<Eigen::Vector3d> path);
void visCorridor(vector<Cube> corridor);
void visBezierTrajectory(MatrixXd polyCoeff, VectorXd time);
void ESDFMapPub(Eigen::Vector3d origin, double resolution, unsigned int size_x, unsigned int size_y, double * cost);
void setViaMakerProperty(const std::string _name);
void trajPlanning(PLANSTYLE style);
double velMapping(const Eigen::Vector3d& pt, double d, double max_v);
void timeAllocation(vector<Cube> & corridor, vector<double> time);
void resetTraj();

bool checkExecTraj();
bool checkCoordObs(Eigen::Vector3d checkPt);

Eigen::VectorXd getStateFromBezier(const MatrixXd & polyCoeff, double t_now, int seg_now );
Eigen::Vector3d getPosFromBezier(const MatrixXd & polyCoeff, double t_now, int seg_now );
wmr_msgs::PolynomialTrajectory getBezierTraj();

void sortPath(vector<Eigen::Vector3d> & path_coord, vector<double> & time);

void laser2Map(const double& pcd_time);

std::string double2string(const double value) {
    std::ostringstream out;
    out.precision(15);
    out << value;
    return out.str();
}

oriTrans orientationTrans_;
queue<Eigen::Vector3d> target_queue_;
bool is_INIT_ = true;

bool isRobotStop(){
    ros::Duration(0.1).sleep();  //暂停0.1s

    const auto my_abs = [](double a){
        return a >= 0 ? a : -a;
    };

    return my_abs(robot_vel_) <= 0.01 && my_abs(robot_avl_) <= 0.01;
}
/**  目标点信息回调函数 --接收目标点  --wp: xyz(m)  **/
void rcvWaypointCallback(const nav_msgs::Path &wp){
    std::string waypoint_type = wp.header.frame_id;
    if(waypoint_type == "HANDLE"){
        /***********清空目标点队列***********/
        if(!target_queue_.empty()){
            queue<Eigen::Vector3d>().swap(target_queue_);
        }

        is_AUTO_    = false;
        has_target_ = false;
        is_emerg_   = false;
        is_INIT_    = true;
        std_msgs::Bool is_auto_mode;
        is_auto_mode.data = false;
        is_Auto_pub_.publish(is_auto_mode);
        return;
    } else if(waypoint_type == "AUTO"){
        if (wp.poses[0].pose.position.z < 0)
            return;
        Eigen::Vector3d pt_tmp;
        pt_tmp << wp.poses[0].pose.position.x,
                  wp.poses[0].pose.position.y,
                  0.01;
        target_queue_.push(pt_tmp);

        is_AUTO_    = true;
        has_target_ = true;
        is_emerg_   = true;
        std_msgs::Bool is_auto_mode;
        is_auto_mode.data = true;
        is_Auto_pub_.publish(is_auto_mode);
    }

    ROS_INFO("[Fast Marching Node] receive the way-points");

    if((is_AUTO_ && has_pos_ && is_INIT_) || isRobotStop() ){
        /*************   第一次转为自动模式   ****************/
        if(!is_INIT_) {  //机器人处于静止状态
            while(target_queue_.size() > 1)
                target_queue_.pop();
        }
        is_INIT_ = false;
        end_pt_  = target_queue_.front();
        trajPlanning(GLOBAL);
    }
}
/**  位置信息回调函数 -- state: -xyz(m), -quanter, -v(m/s) -w(rad/s)  **/
std::list<RobPos> list_pos_;
const int lpos_size_ = 8;
void rcvPositionCallback(const nav_msgs::Odometry &state){
    if (state.header.frame_id != "wmr")
        return ;

    if(std::isnan(state.pose.pose.position.x) ||
       std::isnan(state.pose.pose.position.y) ||
       std::isnan(state.pose.pose.position.z)  ){
        has_pos_ = false;
        return;
    }

    time_now_ = state.header.stamp;
//    ROS_INFO("[btraj-pos tra]: pos TIME %lf, ros TIME %lf", time_now_.toSec(), ros::Time::now().toSec());

    position_lock_.lock();
    start_pt_(0) = state.pose.pose.position.x;
    start_pt_(1) = state.pose.pose.position.y;
//    start_pt_(2) = state.pose.pose.position.z;
    start_pt_(2) = 0.01;

    /******** start vel 为绝对坐标系下的速度 ********/
    start_vel_(0) = state.twist.twist.linear.x;
    start_vel_(1) = state.twist.twist.linear.y;
    start_vel_(2) = state.twist.twist.linear.z;

    start_quan_.w() = state.pose.pose.orientation.w;
    start_quan_.x() = state.pose.pose.orientation.x;
    start_quan_.y() = state.pose.pose.orientation.y;
    start_quan_.z() = state.pose.pose.orientation.z;
    Eigen::Matrix3d DCM;
    Eigen::Vector3d Eular;
    orientationTrans_.Quater2DCM(start_quan_, &DCM);
    orientationTrans_.DCM2Euler(DCM, &Eular);
    //当前机器人的信息
    robot_yaw_ = Eular(2);
    while(robot_yaw_ > 2*M_PI)
        robot_yaw_ -= 2*M_PI;
    while(robot_yaw_ < 0)
        robot_yaw_ += 2*M_PI;

    /************ debug **************/
    if(list_pos_.size() < lpos_size_){
        list_pos_.emplace_back(RobPos(start_pt_, robot_yaw_, time_now_.toSec()));
    } else {
        list_pos_.pop_front();
        list_pos_.emplace_back(RobPos(start_pt_, robot_yaw_, time_now_.toSec()));
    }
    /************ debug **************/

    start_acc_(0) = start_vel_(0) - last_vel_(0);
    start_acc_(1) = start_vel_(1) - last_vel_(1);
    start_acc_(2) = start_vel_(2) - last_vel_(2);
    position_lock_.unlock();

    has_pos_   = true;
//    pos_mark_  = true;
    robot_avl_ = state.twist.twist.angular.z;
    robot_vel_ = sqrt(pow(start_vel_(0), 2) + pow(start_vel_(1), 2));

    last_vel_ = start_vel_;
//    static tf::TransformBroadcaster br;
//    tf::Transform transform;
//    transform.setOrigin( tf::Vector3(start_pt_(0), start_pt_(1), start_pt_(2)));
//    transform.setRotation(tf::Quaternion(0, 0, 0, 1.0));
//    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "wmr"));

    static std::vector<int64_t> last_index;
    auto grid_index = map_generator_->location2Index(start_pt_, GLOBAL);
    if(grid_index.empty()) {}
    else if(last_index.empty()) {
        last_index.swap(grid_index);
    } else {
        if (grid_index[0] == last_index[0] &&
            grid_index[1] == last_index[1] &&
            grid_index[2] == last_index[2]) {}

        last_index.swap(grid_index);
    }

    if(!has_pcd_){
        map_generator_->builtLocalMap(start_pt_);
//        map_generator_->builtLocalMap(q_pos_.front().position);
    }

//    ROS_WARN("local map make : %lf, %lf", start_pt_(0), start_pt_(1));

    visRobotPos(start_pt_, robot_yaw_);
//    visMapPub(std::string("local"));

    if(checkExecTraj()){
        trajPlanning(GLOBAL);
    }
}

/********************* 检测是否在目标点附近 *************************/
double getDistance(const Eigen::Vector3d& a, const Eigen::Vector3d& b){
    double d2 = pow((a[0] - b[0]), 2) + pow((a[1] - b[1]), 2) + pow((a[2] - b[2]), 2);
    return sqrt(d2);
}

void checkReTraj(){
    if(!is_AUTO_ || !has_traj_)
        return;

    position_lock_.lock();
    Eigen::Vector3d pt(start_pt_);
    position_lock_.unlock();

    if(getDistance(pt, end_pt_) < 0.5){
        if(!target_queue_.empty())
            target_queue_.pop();

        if(target_queue_.empty()){
            has_target_ = false;
            is_INIT_    = true;
        } else {
            position_lock_.lock();
            end_pt_ = target_queue_.front();
            position_lock_.unlock();

            trajPlanning(GLOBAL);
        }
    }
}

/*******  获取地面的点云  ***********/
std::vector<float> vground_x_, vground_y_;
bool use_ground_ = false, has_ground_ = false;
std::mutex cloud_lock_;
std::queue<std::vector<float>> q_ground_x_, q_ground_y_;
std::queue<std::vector<float>> q_obj_x_, q_obj_y_;
const unsigned int qcloud_size = 1;
void rcvGroundPointCallback(const sensor_msgs::PointCloud2 &msg){
    if (!use_ground_) use_ground_ = true;     //一旦接收地面点云，表明使用地面点云更新栅格地图
    if(!has_pos_) return;

    cloud_lock_.lock();
    std::vector<float>().swap(vground_x_);
    std::vector<float>().swap(vground_y_);

    pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_xyz(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(msg, *pcd_xyz);
    if (!pcd_xyz->empty()) {
        for (unsigned int i = 0; i < pcd_xyz->size(); i++) {
            vground_x_.push_back(pcd_xyz->points[i].x);
            vground_y_.push_back(pcd_xyz->points[i].y);
        }
    }

    if(q_ground_x_.size() < qcloud_size){
        q_ground_x_.push(vground_x_);
        q_ground_y_.push(vground_y_);
    } else {
        q_ground_x_.pop();
        q_ground_x_.push(vground_x_);
        q_ground_y_.pop();
        q_ground_y_.push(vground_y_);
    }

    cloud_lock_.unlock();

    has_ground_ = true;     //标志接收到一次地面点云信息，每更新一次地图重置一次该标志
}

/*******  获取障碍物的点云  ***********/
/*******  每接收一次位置信息（使用地面点云信息）时  ***********/
/*******  对障碍物点云进行一次处理  ***********/
std::vector<float> vobs_x_, vobs_y_;
void rcvLaserCallback(const sensor_msgs::PointCloud2 &msg){
//    if(!has_pos_ || !pos_mark_ || (use_ground_ && !has_ground_)) return;
    if(!has_pos_ || (use_ground_ && !has_ground_)) return;
    if(!map_generator_->checkPOS(start_pt_)) return;

//    ROS_INFO("[btraj-pcd before]: pcd TIME %lf, ros TIME %lf", msg.header.stamp.toSec(), ros::Time::now().toSec());

    std::vector<float>().swap(vobs_x_);
    std::vector<float>().swap(vobs_y_);

    pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_xyz(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(msg, *pcd_xyz);
    if (!pcd_xyz->empty()) {
        has_pcd_ = true;

        for (unsigned int i = 0; i < pcd_xyz->size(); i++) {
            vobs_x_.push_back(pcd_xyz->points[i].x);
            vobs_y_.push_back(pcd_xyz->points[i].y);
        }

        if(q_obj_x_.size() < qcloud_size){
            q_obj_x_.push(vobs_x_);
            q_obj_y_.push(vobs_y_);
        } else {
            q_obj_x_.pop();
            q_obj_x_.push(vobs_x_);
            q_obj_y_.pop();
            q_obj_y_.push(vobs_y_);
        }

        laser2Map(msg.header.stamp.toSec());
    }
    visMapPub(std::string("local"));
    visMapPub(std::string("world"));
}
/**  雷达信息处理 ： 更新地图、发送EDSF地图 **/
void laser2Map(const double& pcd_time){
    position_lock_.lock();
//    double yaw = 0, _time;
//    Eigen::Vector3d tmp_pt;
//    for(auto iter = list_pos_.begin(); iter != list_pos_.end(); ++iter){
//        if(iter->time <= pcd_time){
//            tmp_pt = iter->position;
//            yaw    = iter->yaw;
//            _time  = iter->time;
//        } else {
//            if(iter == list_pos_.begin()){
//                tmp_pt = iter->position;
//                yaw    = iter->yaw;
//                _time  = iter->time;
//            }
//
//            break;
//        }
//    }
//
//    ROS_INFO("[btraj-pos]: %lf", _time);
//    ROS_INFO("[btraj-point cloud]: %lf", pcd_time);
//    double tmp_x = tmp_pt[0];
//    double tmp_y = tmp_pt[1];
//    map_generator_->builtLocalMap(tmp_pt);

    double tmp_x = start_pt_[0];
    double tmp_y = start_pt_[1];
    double yaw   = robot_yaw_;
    map_generator_->builtLocalMap(start_pt_);
    position_lock_.unlock();
//    ROS_WARN("receive laser data : %lf, %lf", x, y);

    //if ranges[i] is inf, std::pair second is false
    std::vector<std::pair<Eigen::Vector3d, bool >> vec_point;

    Eigen::Vector3d point;

    cloud_lock_.lock();
    for(unsigned int i = 0; i < q_ground_x_.front().size(); i++){
        float& _x = q_ground_x_.front()[i];
        float& _y = q_ground_y_.front()[i];
        double d = sqrt(pow(_x, 2) + pow(_y, 2));
        double a = atan2(_y, _x);
        if(d > 2 * x_local_size_) continue;

        point(0) = tmp_x + d * cos(yaw + a);
        point(1) = tmp_y + d * sin(yaw + a);
        point(2) = 0.1;
        vec_point.emplace_back(std::make_pair(point, false));
    }
//    pos_mark_   = false;
    has_ground_ = false;   //重置标志
    cloud_lock_.unlock();

    for(unsigned int i = 0; i < q_obj_x_.front().size(); i++){
        float& _x = q_obj_x_.front()[i];
        float& _y = q_obj_y_.front()[i];
        double d = sqrt(pow(_x, 2) + pow(_y, 2));
        double a = atan2(_y, _x);
        if(d > 1.5 * x_local_size_) continue;

        point(0) = tmp_x + d * cos(yaw + a);
        point(1) = tmp_y + d * sin(yaw + a);
        point(2) = 0.1;
        vec_point.emplace_back(std::make_pair(point, true));
    }

    ROS_INFO("[btraj-pos]: %lf, %0.3f, %0.3f, %0.3f", time_now_.toSec(), tmp_x, tmp_y, yaw*180/M_PI);
    map_generator_->refreshMap(tmp_x, tmp_y, vec_point);
    map_generator_->getGlobalMap(global_map_);
    corridor_2d_->refreshMap(global_map_, map_generator_->getMapSize());
    has_map_ = true;

    /***** 发送势场地图EDSF ******/
    float oob_value = INFINITY;

    map_generator_->getLocalMap(local_map_);
    auto EDT = local_map_->ExtractDistanceField(oob_value);
    const unsigned int size_x = global_map_->GetNumXCells();
    const unsigned int size_y = global_map_->GetNumYCells();
    if(size_x * size_y == 0) return;
//    const unsigned int size_x = local_map->GetNumXCells();
//    const unsigned int size_y = local_map->GetNumYCells();
    //make EDSF map nav_msgs::OccupancyGrid data[width*high]
    double *cost = new double[size_x * size_y]{0};
    Eigen::Vector3d pt, map_origin;
    map_generator_->getMapOrigin(map_origin);
//    auto map_origin = map_generator_->getLocalOrigin();
    for (unsigned int j = 0; j < size_y; j++) {
        for (unsigned int i = 0; i < size_x; i++) {
            pt << (i + 0.5) * resolution_ + map_origin(0),
                    (j + 0.5) * resolution_ + map_origin(1),
                    (0 + 0.5) * resolution_ + map_origin(2);

            Eigen::Vector3i index   = local_map_->LocationToGridIndex(pt);
            Eigen::Vector3i g_index = global_map_->LocationToGridIndex(pt);

            if(local_map_->Inside(index))
            {
                double d = sqrt(EDT.GetImmutable(index).first.distance_square) * resolution_;
                d = (d == INFINITY ? 2 : d);
                // set EDSF map nav_msgs::OccupancyGrid data[i]
                cost[i * size_y + j] = d * inv_resolution_;
            }
            else if(global_map_->Inside(g_index)){
                int64_t ix = g_index[0];
                int64_t iy = g_index[1];
                int64_t iz = g_index[2];
                auto cell = global_map_->Get(ix, iy, iz);
                if(cell.first.occupancy > 0.5) {
                    cost[i * size_y + j] = 0;
                } else {
                    cost[i * size_y + j] = 1;
                }
            }
        }
    }

    Eigen::Vector3i index_rob   = global_map_->LocationToGridIndex(start_pt_);
    if(cost[index_rob[0] * size_y + index_rob[1]] <= 0){
        cost[index_rob[0] * size_y + index_rob[1]] = resolution_;
    }

    //publish the EDSF map
    ESDFMapPub(map_origin, global_map_->GetResolution(), size_x, size_y, cost);
    delete[](cost);
    cost  = nullptr;
}

void rvcTrajStimeCallback(std_msgs::Header msg){
    start_time_ = msg.stamp;
}

/**  传入参数--无可通行路径(flase) --请求重置路径(true) ***/
/**  (-false) 请求规划器清楚所有目标点 ***/
void restartTrajCallback(std_msgs::Bool msg){

    if (msg.data == true && target_queue_.size() > 1) {
        position_lock_.lock();
        target_queue_.pop();
        end_pt_ = target_queue_.front();
        position_lock_.unlock();

        trajPlanning(GLOBAL);
    } else if(msg.data == false){
        ROS_INFO("[btraj node]: reTraj -----");
        trajPlanning(GLOBAL);
    }
    else {
        queue<Eigen::Vector3d>().swap(target_queue_);
        has_target_ = false;
        is_INIT_ = true;
    }

}

int main(int argc, char **argv){
    ros::init(argc, argv, "btraj_node");
    ros::NodeHandle nh("~");

    ros::Subscriber pts_sub = nh.subscribe("/waypoints", 10, rcvWaypointCallback);
    ros::Subscriber pos_sub = nh.subscribe("/pos",       1,  rcvPositionCallback);
    ros::Subscriber occ_sub = nh.subscribe("/obj_pcd",   1,  rcvLaserCallback);
    ros::Subscriber grp_sub = nh.subscribe("/obj_ground",  1, rcvGroundPointCallback);
    ros::Subscriber res_sub = nh.subscribe("/restart_traj", 1, restartTrajCallback);
    ros::Subscriber traj_Stime_sub = nh.subscribe("/traj_start_time",  10, rvcTrajStimeCallback);

    ESDF_map_pub_     = nh.advertise<nav_msgs::OccupancyGrid>("/ESDF_map",   1000);
    vis_map_pub_      = nh.advertise<visualization_msgs::Marker>("/vis_map", 1000);
    vis_traj_pub_     = nh.advertise<visualization_msgs::Marker>("/trajectory_vis",    1);
    vis_rpos_pub_     = nh.advertise<visualization_msgs::Marker>("/robot_pos",         10);
    checkTraj_vis_pub_ = nh.advertise<visualization_msgs::Marker>("/check_trajectory", 1);
    stopTraj_vis_pub_  = nh.advertise<visualization_msgs::Marker>("/stop_trajectory",  1);
    vis_path_pub_     = nh.advertise<visualization_msgs::MarkerArray>("/vis_path",     10);
    vis_corridor_pub_ = nh.advertise<visualization_msgs::MarkerArray>("/vis_corridor", 1);

    traj_pub_         = nh.advertise<wmr_msgs::PolynomialTrajectory>("/trajectory",    10);
    is_Auto_pub_      = nh.advertise<std_msgs::Bool>("/is_auto_mode",  2);

    bool is_mapping = false;
    std::string map_path;

    nh.param("map/path",         map_path,
             std::string("/home/jg/catkin_ws/btraj/wmr_bezier_planner/map/information_department_index.map"));
    nh.param("map/resolution",   resolution_, 0.2);
    nh.param("map/margin",       map_margin_, 0.5);
    nh.param("map/inflate",      inflate_,    0.4);
    nh.param("map/is_mapping",   is_mapping, false);

    nh.param("map/x_size",       x_size_, 50.0);
    nh.param("map/y_size",       y_size_, 50.0);
    nh.param("map/z_size",       z_size_, 5.0 );

    nh.param("map/x_local_size", x_local_size_, 10.0);
    nh.param("map/y_local_size", y_local_size_, 10.0);
    nh.param("map/z_local_size", z_local_size_, 5.0 );

    nh.param("planning/max_vel",       MAX_Vel_,  1.0);
    nh.param("planning/max_acc",       MAX_Acc_,  1.0);
    nh.param("planning/max_inflate",   max_inflate_iter_, 100);
    nh.param("planning/step_length",   step_length_,      2);
    nh.param("planning/cube_margin",   cube_margin_,      0.2);
    nh.param("planning/check_horizon", check_horizon_,10.0);
    nh.param("planning/stop_horizon",  stop_horizon_,  5.0);
    nh.param("planning/is_limit_vel",  is_limit_vel_,  false);
    nh.param("planning/is_limit_acc",  is_limit_acc_,  false);

    nh.param("optimization/min_order",  minimize_order_, 3.0);
    nh.param("optimization/poly_order", traj_order_,     10);

    nh.param("vis/vis_traj_width", vis_traj_width_, 0.15);
    nh.param("vis/is_proj_cube",   is_proj_cube_,   true);
    nh.param("planning/retraj_program",   is_retraj_prog_,   false);

    inv_resolution_ = 1.0 / resolution_;

    map_generator_ = std::make_shared<MAP_GENERATOR>(map_path ,x_local_size_, y_local_size_, resolution_, is_mapping, map_margin_, MAX_Vel_, inflate_);
    has_map_ = true;
    map_generator_->getGlobalMap(global_map_);
    corridor_2d_ = std::make_shared<CORRIDOR_GENERATOR>(global_map_, map_generator_->getMapSize(),max_inflate_iter_, step_length_, cube_margin_);

    Bernstein bernstein;
    if(bernstein.setParam(3, 12, minimize_order_) == -1)
        ROS_ERROR(" The trajectory order is set beyond the library's scope, please re-set ");

    MQM_ = bernstein.getMQM()[traj_order_];
    FM_  = bernstein.getFM()[traj_order_];
    C_   = bernstein.getC()[traj_order_];
    Cv_  = bernstein.getC_v()[traj_order_];
    Ca_  = bernstein.getC_a()[traj_order_];
    Cj_  = bernstein.getC_j()[traj_order_];

    ros::Rate rate(50);
    int count = 100;
    while(ros::ok())
    {
        if(count == 0){
            visMapPub(std::string("world"));
            checkReTraj();
            count = 100;
        }
        count--;

        ros::spinOnce();
        rate.sleep();
    }

    return 0;

}
/*****  rviz:显示机器人位置  ****/
void visRobotPos(Eigen::Vector3d pt, double yaw){
    visualization_msgs::Marker pt_vis;

    pt_vis.header.stamp       = time_now_;
    pt_vis.header.frame_id    = "odom";

    pt_vis.ns = "robot/position";
    pt_vis.id = 0;
    pt_vis.type = visualization_msgs::Marker::ARROW;

    pt_vis.action = visualization_msgs::Marker::DELETE;
    vis_rpos_pub_.publish(pt_vis);
//    _checkTraj_vis_pub.publish(traj_vis);
//    _stopTraj_vis_pub.publish(traj_vis);

    pt_vis.action = visualization_msgs::Marker::ADD;
    const float _lenght = 0.8;
    pt_vis.scale.x = 0.3;
    pt_vis.scale.y = 0.4;
    pt_vis.scale.z = _lenght;
//    pt_vis.pose.orientation.x = 0.0;
//    pt_vis.pose.orientation.y = 0.0;
//    pt_vis.pose.orientation.z = 0.0;
//    pt_vis.pose.orientation.w = 1.0;
    pt_vis.color.r = 1.0;
    pt_vis.color.g = 1.0;
    pt_vis.color.b = 0.8;
    pt_vis.color.a = 1.0;

    geometry_msgs::Point p1, p2;
    p1.x = pt(0) - 0.5*_lenght * cos(yaw);
    p1.y = pt(1) - 0.5*_lenght * sin(yaw);
    p1.z = pt(2);
    p2.x = pt(0) + 0.5*_lenght * cos(yaw);
    p2.y = pt(1) + 0.5*_lenght * sin(yaw);
    p2.z = pt(2);
    pt_vis.points.push_back(p1);
    pt_vis.points.push_back(p2);

    vis_rpos_pub_.publish(pt_vis);
}

//sdf_tools::CollisionMapGrid* maptmp = new sdf_tools::CollisionMapGrid();
/*****  rviz:显示地图  ****/
void visMapPub(const std::string& _name){
    setViaMakerProperty(_name);

    map_marker_.header.frame_id = "odom";
    map_marker_.header.stamp = time_now_;
    map_marker_.id = 0;

    map_marker_.points.clear();
    geometry_msgs::Point point;
    std::shared_ptr<sdf_tools::CollisionMapGrid> maptmp;
    if (_name == "world"){
        map_generator_->getGlobalMap(global_map_);
        maptmp = global_map_;
    }
    else if (_name == "local"){
        map_generator_->getLocalMap(local_map_);
        maptmp = local_map_;
    }
    if(!maptmp->IsInitialized()) return;
    for (int64_t x = 0; x < maptmp->GetNumXCells(); ++x)
        for (int64_t y = 0; y < maptmp->GetNumYCells(); ++y) {
            auto cell = maptmp->Get(x, y, 0);
            if (cell.first.occupancy > 0.0) {
                auto pt = maptmp->GridIndexToLocation(x, y, 0);
                point.x = pt[0];
                point.y = pt[1];
                point.z = pt[2];
                map_marker_.points.push_back(point);
            }
        }

    //map boundery
    auto vec = map_generator_->getMapSize();
    geometry_msgs::Point p1, p2;
    for(double xmar = vec[0]; xmar <= vec[1]; xmar += resolution_){
        p1.x = xmar;    p2.x = xmar;
        p1.y = vec[2];  p2.y = vec[3];
        p1.z = 0;       p2.z = 0;
        map_marker_.points.push_back(p1);
        map_marker_.points.push_back(p2);
    }
    for(double ymar = vec[2]; ymar <= vec[3]; ymar += resolution_){
        p1.x = vec[0];    p2.x = vec[1];
        p1.y = ymar;      p2.y = ymar;
        p1.z = 0;         p2.z = 0;
        map_marker_.points.push_back(p1);
        map_marker_.points.push_back(p2);
    }

    vis_map_pub_.publish(map_marker_);
}
/*****  rviz:设置地图属性  ****/
void setViaMakerProperty(const std::string _name) {
    if (_name == "world"){
        /*decide from which view we can see the marker*/
        map_marker_.ns = _name;
        /*set marker type*/
        map_marker_.type = visualization_msgs::Marker::SPHERE_LIST;
        /*decide if the marker will be enlarge*/
        map_marker_.scale.x = 0.1;
        map_marker_.scale.y = 0.1;
        map_marker_.scale.z = 0.1;
        map_marker_.pose.orientation.w = 1;
        map_marker_.pose.orientation.x = 0;
        map_marker_.pose.orientation.y = 0;
        map_marker_.pose.orientation.z = 0;
        /*decide the color of the marker*/
        map_marker_.color.a = 1.0; // Don't forget to set the alpha!
        map_marker_.color.r = 1.0;
        map_marker_.color.g = 1.0;
        map_marker_.color.b = 1.0;
        /*set marker action*/
        map_marker_.action = visualization_msgs::Marker::ADD;
    }
    else if(_name == "local"){
        /*decide from which view we can see the marker*/
        map_marker_.ns = _name;
        /*set marker type*/
        map_marker_.type = visualization_msgs::Marker::SPHERE_LIST;
        /*decide if the marker will be enlarge*/
        map_marker_.scale.x = 0.15;
        map_marker_.scale.y = 0.15;
        map_marker_.scale.z = 0.15;
        map_marker_.pose.orientation.w = 1;
        map_marker_.pose.orientation.x = 0;
        map_marker_.pose.orientation.y = 0;
        map_marker_.pose.orientation.z = 0;
        /*decide the color of the marker*/
        map_marker_.color.a = 1.0; // Don't forget to set the alpha!
        map_marker_.color.r = 1.0;
        map_marker_.color.g = 0.0;
        map_marker_.color.b = 0.0;
        /*set marker action*/
        map_marker_.action = visualization_msgs::Marker::ADD;
    }
}
/*****  轨迹规划  ****/
void trajPlanning(PLANSTYLE style){
    if(!has_map_ || !has_pos_ || !has_target_)
        return;

    if(style == GLOBAL) {
        position_lock_.lock();
        ros::Time time_1 = ros::Time::now();
        float oob_value = INFINITY;
//        float oob_value = 0;
        map_generator_->getGlobalMap(global_map_);
        map_generator_->getLocalMap(local_map_);
        auto EDT = local_map_->ExtractDistanceField(oob_value);

        ros::Time time_2 = ros::Time::now();
        ROS_WARN("time in generate EDT is %f", (time_2 - time_1).toSec());

        unsigned int idx;
        double max_vel = MAX_Vel_ * 0.5;
        vector<unsigned int> obs;
        Eigen::Vector3d pt;
        vector<int64_t> pt_idx;
        double flow_vel;

        const unsigned int size_x = global_map_->GetNumXCells();
        const unsigned int size_y = global_map_->GetNumYCells();
        const unsigned int size_z = global_map_->GetNumZCells();
        Eigen::Vector3d map_origin;
        map_generator_->getMapOrigin(map_origin);
        //make EDSF map nav_msgs::OccupancyGrid data[width*high]
        double *cost = new double[size_x * size_y]{0};

        Coord3D dimsize {size_x, size_y, size_z};
        FMGrid3D grid_fmm(dimsize);

        for(unsigned int k = 0; k < size_z; k++){
            for(unsigned int j = 0; j < size_y; j++){
                for(unsigned int i = 0; i < size_x; i++){
                    //grid_fmm's grid index : idx
                    idx = k * size_y * size_x + j * size_x + i;
                    pt << (i + 0.5) * resolution_ + map_origin(0),
                          (j + 0.5) * resolution_ + map_origin(1),
                          (k + 0.5) * resolution_ + map_origin(2);

                    Eigen::Vector3i index   = local_map_->LocationToGridIndex(pt);
                    Eigen::Vector3i g_index = global_map_->LocationToGridIndex(pt);

                    if(local_map_->Inside(index))
                    {
                        double d = sqrt(EDT.GetImmutable(index).first.distance_square) * resolution_;
                        d = (d == INFINITY ? 2 : d);
                        flow_vel = velMapping(pt, d, max_vel);
                        // set EDSF map nav_msgs::OccupancyGrid data[i]
                        cost[i * size_y + j] = d * inv_resolution_;
                    }
                    else if(global_map_->Inside(g_index)){
                        int64_t ix = g_index[0];
                        int64_t iy = g_index[1];
                        int64_t iz = g_index[2];
                        auto cell = global_map_->Get(ix, iy, iz);
                        if(cell.first.occupancy > 0.5) {
                            flow_vel = 0;
                            cost[i * size_y + j] = 0;
                        } else {
                            flow_vel = max_vel;
                            cost[i * size_y + j] = 1;
                        }

                    }
                    else
                        flow_vel = max_vel;

                    if( j == 0 || j == (size_y - 1) || i == 0 || i == (size_x - 1) )
                        flow_vel = 0.0;

                    grid_fmm[idx].setOccupancy(flow_vel);
                    if (grid_fmm[idx].isOccupied())
                        obs.push_back(idx);
                }
            }
        }

        //publish the EDSF map
        ESDFMapPub(map_origin, global_map_->GetResolution(), size_x, size_y, cost);
        delete[](cost);
        cost  = nullptr;

        grid_fmm.setOccupiedCells(std::move(obs));
        grid_fmm.setLeafSize(resolution_);
        Eigen::Vector3d startIdx3d = (start_pt_ - map_origin) * inv_resolution_;
        Eigen::Vector3d endIdx3d   = (end_pt_   - map_origin) * inv_resolution_;

        Coord3D goal_point = {(unsigned int)startIdx3d[0], (unsigned int)startIdx3d[1], (unsigned int)startIdx3d[2]};
        Coord3D init_point = {(unsigned int)endIdx3d[0],   (unsigned int)endIdx3d[1],   (unsigned int)endIdx3d[2]};

        unsigned int startIdx;
        vector<unsigned int> startIndices;
        grid_fmm.coord2idx(init_point, startIdx);
        startIndices.push_back(startIdx);

        unsigned int goalIdx;
        grid_fmm.coord2idx(goal_point, goalIdx);
        grid_fmm[goalIdx].setOccupancy(max_vel);

        Solver<FMGrid3D>* fm_solver = new FMMStar<FMGrid3D>("FMM*_Dist", TIME); // LSM, FMM

        fm_solver->setEnvironment(&grid_fmm);
        fm_solver->setInitialAndGoalPoints(startIndices, goalIdx);

        ros::Time time_bef_fm = ros::Time::now();
        if (fm_solver->compute(max_vel) == -1) {
            ROS_WARN("[Fast Marching Node] No path can be found");
            resetTraj();
            has_traj_ = false;
            traj_pub_.publish(traj_);

            queue<Eigen::Vector3d>().swap(target_queue_);
            has_target_ = false;
            is_INIT_ = true;
            position_lock_.unlock();
            return;
        }
        ros::Time time_aft_fm = ros::Time::now();
        ROS_WARN("[Fast Marching Node] Time in Fast Marching computing is %f", (time_aft_fm - time_bef_fm).toSec() );

        Path3D path3D;
        vector<double> path_vels, time;
        GradientDescent< FMGrid3D > grad3D;
        grid_fmm.coord2idx(goal_point, goalIdx);

        if (grad3D.gradient_descent(grid_fmm, goalIdx, path3D, path_vels, time) == -1) {
            ROS_WARN("[Fast Marching Node] FMM failed, valid path not exists");
            if (has_traj_ && is_emerg_) {
                resetTraj();
                traj_pub_.publish(traj_);
                queue<Eigen::Vector3d>().swap(target_queue_);
                has_target_ = false;
                has_traj_ = false;
                is_INIT_  = true;
            }
            position_lock_.unlock();
            return;
        }

        std::vector<Eigen::Vector3d> path_coord;
        path_coord.push_back(start_pt_);

        double coord_x, coord_y, coord_z;
        auto map_size = map_generator_->getMapSize();
        for( int i = 0; i < (int)path3D.size(); i++){
            coord_x = max(min( (path3D[i][0]+0.5) * resolution_ + map_origin(0), map_size[1]), map_size[0]);
            coord_y = max(min( (path3D[i][1]+0.5) * resolution_ + map_origin(1), map_size[3]), map_size[2]);
            coord_z = 0;

            Eigen::Vector3d pt(coord_x, coord_y, coord_z);
            path_coord.push_back(pt);
        }
        visPath(path_coord);

        ros::Time time_bef_corridor = ros::Time::now();
        sortPath(path_coord, time);
        auto corridor = corridor_2d_->corridorGeneration(path_coord, time);
        ros::Time time_aft_corridor = ros::Time::now();
        ROS_WARN("Time consume in corridor generation is %f", (time_aft_corridor - time_bef_corridor).toSec());

        timeAllocation(corridor, time);
        visCorridor(corridor);
        delete fm_solver;
        fm_solver = nullptr;

        Eigen::MatrixXd pos = Eigen::MatrixXd::Zero(2,3);
        Eigen::MatrixXd vel = Eigen::MatrixXd::Zero(2,3);
        Eigen::MatrixXd acc = Eigen::MatrixXd::Zero(2,3);

        pos.row(0) = start_pt_;
        pos.row(1) = end_pt_;
        vel.row(0) = start_vel_;
        acc.row(0) = start_acc_;
        position_lock_.unlock();

        double obj;
        ros::Time time_bef_opt = ros::Time::now();
        if(trajectory_generator_.BezierPloyCoeffGeneration
                ( corridor, MQM_, pos, vel, acc, MAX_Vel_, MAX_Acc_, traj_order_, minimize_order_,
                  cube_margin_, is_limit_vel_, is_limit_acc_, obj, bezier_coeff_) == -1 )
        {
            ROS_WARN("Cannot find a feasible and optimal solution, somthing wrong with the optimazation solver\n");

            if(has_traj_ && is_emerg_)
            {
                resetTraj();
                traj_pub_.publish(traj_);
                queue<Eigen::Vector3d>().swap(target_queue_);
                has_target_ = false;
                is_INIT_  = true;
                has_traj_ = false;
            }
        }
        else
        {
            seg_num_ = corridor.size();
            seg_time_.resize(seg_num_);

            for(int i = 0; i < seg_num_; i++)
                seg_time_(i) = corridor[i].t;

            is_emerg_ = false;
            has_traj_ = true;

            traj_ = getBezierTraj();
            traj_pub_.publish(traj_);
            traj_id_ ++;
            visBezierTrajectory(bezier_coeff_, seg_time_);
        }

        ros::Time time_aft_opt = ros::Time::now();

        ROS_WARN("The objective of the program is %f", obj);
        ROS_WARN("The time consumation of the program is %f\n", (time_aft_opt - time_bef_opt).toSec());
    }

}

void resetTraj(){
    traj_.action = wmr_msgs::PolynomialTrajectory::ACTION_ABORT;

    traj_.num_segment = 0;
    traj_.coef_x.clear();
    traj_.coef_y.clear();
    traj_.coef_z.clear();
}
/*****  规划：对路径进行时间分配  ****/
void timeAllocation(vector<Cube> & corridor, vector<double> time){
    vector<double> tmp_time;

    for(int i  = 0; i < (int)corridor.size() - 1; i++)
    {
        double duration  = (corridor[i].t - corridor[i+1].t);
        tmp_time.push_back(duration);
    }
    double lst_time = corridor.back().t;
    tmp_time.push_back(lst_time);

    vector<Eigen::Vector3d> points;
    points.push_back (start_pt_);
    for(int i = 1; i < (int)corridor.size(); i++)
        points.push_back(corridor[i].center);

    points.push_back (end_pt_);

    const double Expect_Vel = MAX_Vel_ * 0.6;
    const double Expect_Acc = MAX_Acc_ * 0.6;
    double _vel, _acc;

    Eigen::Vector3d initv = start_vel_;
    for(int i = 0; i < (int)points.size() - 1; i++)
    {
        double dtxyz;

        Eigen::Vector3d p0   = points[i];
        Eigen::Vector3d p1   = points[i + 1];
        Eigen::Vector3d d    = p1 - p0;
        Eigen::Vector3d v0(0.0, 0.0, 0.0);

        if( i == 0) v0 = initv;

        double D    = d.norm();    //求范数，即长度
        //velocity vector maps on distance vector
        double V0   = v0.dot(d / D);   //点乘，即速度在位移方向的投影
        double aV0  = fabs(V0);
        //避免速度过快
        _vel = (i == 0 && D <= 5) ? 0.5*Expect_Vel : Expect_Vel;
        _acc = (i == 0 && D <= 5) ? 0.7*Expect_Acc : Expect_Acc;

        double acct = (_vel - V0) / _acc * ((_vel > V0)?1:-1);   // 加(减)速至预期速度_Vel时间
        double accd = V0 * acct + (_acc * acct * acct / 2) * ((_vel > V0)?1:-1);  // 加速距离
        double dcct = _vel / _acc;  //从预期速度_Vel减速至0所需时间
        double dccd = _acc * dcct * dcct / 2;  //减速距离

        if (D < aV0 * aV0 / (2 * _acc))     //speed down
        {
            double t1 = (V0 < 0)?2.0 * aV0 / _acc:0.0;
            double t2 = aV0 / _acc;
            dtxyz     = t1 + t2;
        }
        else if (D < accd + dccd)       //speed up, and then speed down
        {
            double t1 = (V0 < 0)?2.0 * aV0 / _acc:0.0;
            double t2 = (-aV0 + sqrt(aV0 * aV0 + _acc * D - aV0 * aV0 / 2)) / _acc;
            double t3 = (aV0 + _acc * t2) / _acc;
            dtxyz     = t1 + t2 + t3;
        }
        else        //speed up, then uniform speed, speed down
        {
            double t1 = acct;
            double t2 = (D - accd - dccd) / _vel;
            double t3 = dcct;
            dtxyz     = t1 + t2 + t3;
        }

        if(dtxyz < tmp_time[i] * 0.5 || dtxyz * 0.8 > tmp_time[i]) {
            tmp_time[i] = dtxyz; // if FM given time in this segment is rediculous long, use the new value
        }
        if(i == 0 && tmp_time[i] < dtxyz){
            tmp_time[i] = dtxyz;
        }
    }

    for(int i = 0; i < (int)corridor.size(); i++){
        corridor[i].t = tmp_time[i];
    }
}

void sortPath(vector<Eigen::Vector3d> & path_coord, vector<double> & time){
    vector<Eigen::Vector3d> path_tmp;
    vector<double> time_tmp;

    for (int i = 0; i < (int)path_coord.size(); i += 1)
    {
        if( i )
            if( std::isinf(time[i]) || time[i] == 0.0 || time[i] == time[i-1] )
                continue;

        if( (path_coord[i] - end_pt_).norm() < 0.2)
            break;

        path_tmp.push_back(path_coord[i]);
        time_tmp.push_back(time[i]);
    }
    path_coord = path_tmp;
    time       = time_tmp;
}

double velMapping(const Eigen::Vector3d& pt, double d, double max_v){
    double vel;
    const double len_map = 1.2;

    const auto get_vel = [&](double tmp_dis, double len){
        double invlen = 1 / len;
        if( tmp_dis <= 0.25 * len)
            return 2.0 * tmp_dis * invlen * tmp_dis * invlen;
        else if(tmp_dis > 0.25 * len && tmp_dis <= 0.75 * len)
            return 1.5 * tmp_dis * invlen - 0.25;
        else if(tmp_dis > 0.75 * len && tmp_dis <= 1.0 * len)
            return - 2.0 * (tmp_dis * invlen - 1.0) * (tmp_dis * invlen - 1.0) + 1;
        else
            return 1.0;
    };

    const auto get_dis = [](Eigen::Vector3d a, Eigen::Vector3d b){
        double dis = pow((a[0] - b[0]), 2) + pow((a[1] - b[1]), 2) + pow((a[2] - b[2]), 2);
        return sqrt(dis);
    };

    const double dis_2_rob = get_dis(pt, start_pt_);
    const double dis_2_end = get_dis(pt, end_pt_);
    double dis_tmp = min(dis_2_rob, dis_2_end);

    vel = (d <= 0 || dis_tmp > len_map) ? get_vel(d, len_map) : get_vel(dis_tmp, len_map);

    return vel * max_v;
}
/*****  设置地图属性 且发布  ****/
void ESDFMapPub(Eigen::Vector3d origin, double resolution, unsigned int size_x, unsigned int size_y, double * cost){
    nav_msgs::OccupancyGrid map;

    map.header.frame_id = "odom";
    map.header.stamp    = time_now_;
    map.info.resolution = resolution;
    map.info.width      = size_x;
    map.info.height     = size_y;

//    auto p_origin = global_map_->GridIndexToLocation(0, 0, 0);
//    map.info.origin.position.x = p_origin[0];
//    map.info.origin.position.y = p_origin[1];
//    map.info.origin.position.z = p_origin[2];
    map.info.origin.position.x = origin[0];
    map.info.origin.position.y = origin[1];
    map.info.origin.position.z = origin[2];
    map.info.origin.orientation.w = 1;
    map.info.origin.orientation.x = 0;
    map.info.origin.orientation.y = 0;
    map.info.origin.orientation.z = 0;
    map.data.resize(map.info.width * map.info.height);

    for (int i = 0; i < (int)(size_x * size_y); ++i) {
        map.data[i] = ceil(cost[i]);
    }

    ESDF_map_pub_.publish(map);
}

/*****  rviz:显示路径  ****/
visualization_msgs::MarkerArray path_vis;
void visPath(std::vector<Eigen::Vector3d> path){
    for(auto & mk: path_vis.markers)
        mk.action = visualization_msgs::Marker::DELETE;

    vis_path_pub_.publish(path_vis);
    path_vis.markers.clear();

    visualization_msgs::Marker mk;
    mk.header.frame_id = "odom";
    mk.header.stamp = time_now_;
    mk.ns = "b_traj/fast_marching_path";
    mk.type = visualization_msgs::Marker::CUBE;
    mk.action = visualization_msgs::Marker::ADD;

    mk.pose.orientation.x = 0.0;
    mk.pose.orientation.y = 0.0;
    mk.pose.orientation.z = 0.0;
    mk.pose.orientation.w = 1.0;
    mk.color.a = 0.6;
    mk.color.r = 0.8;
    mk.color.g = 0.8;
    mk.color.b = 0.0;

    int idx = 0;
    for(int i = 0; i < int(path.size()); i++){
        mk.id = idx;

        mk.pose.position.x = path[i](0);
        mk.pose.position.y = path[i](1);
        mk.pose.position.z = path[i](2);

        mk.scale.x = resolution_;
        mk.scale.y = resolution_;
        mk.scale.z = resolution_;

        idx ++;
        path_vis.markers.push_back(mk);
    }

    vis_path_pub_.publish(path_vis);
}
/*****  rviz:显示corridor  ****/
visualization_msgs::MarkerArray cube_vis;
void visCorridor(vector<Cube> corridor)
{
    for(auto & mk: cube_vis.markers)
        mk.action = visualization_msgs::Marker::DELETE;

    vis_corridor_pub_.publish(cube_vis);

    cube_vis.markers.clear();

    visualization_msgs::Marker mk;
    mk.header.frame_id = "odom";
    mk.header.stamp = time_now_;
    mk.ns = "corridor";
    mk.type = visualization_msgs::Marker::CUBE;
    mk.action = visualization_msgs::Marker::ADD;

    mk.pose.orientation.x = 0.0;
    mk.pose.orientation.y = 0.0;
    mk.pose.orientation.z = 0.0;
    mk.pose.orientation.w = 1.0;

    mk.color.a = 0.4;
    mk.color.r = 1.0;
    mk.color.g = 1.0;
    mk.color.b = 1.0;

    int idx = 0;
    for(int i = 0; i < int(corridor.size()); i++)
    {
        mk.id = idx;

        mk.pose.position.x = (corridor[i].vertex(0, 0) + corridor[i].vertex(3, 0) ) / 2.0;
        mk.pose.position.y = (corridor[i].vertex(0, 1) + corridor[i].vertex(1, 1) ) / 2.0;

        if(is_proj_cube_)
            mk.pose.position.z = 0.0;
        else
            mk.pose.position.z = (corridor[i].vertex(0, 2) + corridor[i].vertex(4, 2) ) / 2.0;

        mk.scale.x = (corridor[i].vertex(0, 0) - corridor[i].vertex(3, 0) );
        mk.scale.y = (corridor[i].vertex(1, 1) - corridor[i].vertex(0, 1) );

        if(is_proj_cube_)
            mk.scale.z = 0.05;
        else
            mk.scale.z = (corridor[i].vertex(0, 2) - corridor[i].vertex(4, 2) );

        idx ++;
        cube_vis.markers.push_back(mk);
    }

    vis_corridor_pub_.publish(cube_vis);
}
/*****  rviz:显示轨迹  ****/
void visBezierTrajectory(MatrixXd polyCoeff, VectorXd time)
{
    visualization_msgs::Marker traj_vis;

    traj_vis.header.stamp       = time_now_;
    traj_vis.header.frame_id    = "odom";

    traj_vis.ns = "trajectory/trajectory";
    traj_vis.id = 0;
    traj_vis.type = visualization_msgs::Marker::SPHERE_LIST;

    traj_vis.action = visualization_msgs::Marker::DELETE;
    checkTraj_vis_pub_.publish(traj_vis);
    stopTraj_vis_pub_.publish(traj_vis);

    traj_vis.action = visualization_msgs::Marker::ADD;
    traj_vis.scale.x = vis_traj_width_;
    traj_vis.scale.y = vis_traj_width_;
    traj_vis.scale.z = vis_traj_width_;
    traj_vis.pose.orientation.x = 0.0;
    traj_vis.pose.orientation.y = 0.0;
    traj_vis.pose.orientation.z = 0.0;
    traj_vis.pose.orientation.w = 1.0;
    traj_vis.color.r = 0.6;
    traj_vis.color.g = 0.0;
    traj_vis.color.b = 0.0;
    traj_vis.color.a = 0.5;

    double traj_len = 0.0;
    int count = 0;
    Eigen::Vector3d cur, pre;
    cur.setZero();
    pre.setZero();

    traj_vis.points.clear();

    Eigen::Vector3d state;
    geometry_msgs::Point pt;

    int segment_num  = polyCoeff.rows();
    for(int i = 0; i < segment_num; i++ ){
        for (double t = 0.0; t < 1.0; t += 0.05 / time(i), count += 1){
            state = getPosFromBezier( polyCoeff, t, i );
            cur(0) = pt.x = time(i) * state(0);
            cur(1) = pt.y = time(i) * state(1);
            cur(2) = pt.z = time(i) * state(2);
            traj_vis.points.push_back(pt);

            if(i == 0 && t == 0.0) {
                traj_start_pt_(0) = pt.x;
                traj_start_pt_(1) = pt.y;
                traj_start_pt_(2) = pt.z;
            }

            if (count) traj_len += (pre - cur).norm();
            pre = cur;
        }
    }

    ROS_INFO("[GENERATOR] The length of the trajectory; %.3lfm.", traj_len);
    vis_traj_pub_.publish(traj_vis);
}
/*****  轨迹：获取轨迹某一点的位置信息  ****/
Eigen::Vector3d getPosFromBezier(const MatrixXd & polyCoeff, double t_now, int seg_now )
{
    Eigen::Vector3d ret = Eigen::VectorXd::Zero(3);
    Eigen::VectorXd ctrl_now = polyCoeff.row(seg_now);
    int ctrl_num1D = polyCoeff.cols() / 3;

    for(int i = 0; i < 3; i++)
        for(int j = 0; j < ctrl_num1D; j++)
            ret(i) += C_(j) * ctrl_now(i * ctrl_num1D + j) * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j) );

    return ret;
}
/*****  轨迹：获取轨迹某一点的状态信息  ****/
Eigen::VectorXd getStateFromBezier(const MatrixXd & polyCoeff, double t_now, int seg_now )
{
    Eigen::VectorXd ret = Eigen::VectorXd::Zero(12);

    Eigen::VectorXd ctrl_now = polyCoeff.row(seg_now);
    int ctrl_num1D = polyCoeff.cols() / 3;

    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < ctrl_num1D; j++){
            ret[i] += C_(j) * ctrl_now(i * ctrl_num1D + j) * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j) );

            if(j < ctrl_num1D - 1 )
                ret[i+3] += Cv_(j) * traj_order_
                            * ( ctrl_now(i * ctrl_num1D + j + 1) - ctrl_now(i * ctrl_num1D + j))
                            * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j - 1) );

            if(j < ctrl_num1D - 2 )
                ret[i+6] += Ca_(j) * traj_order_ * (traj_order_ - 1)
                            * ( ctrl_now(i * ctrl_num1D + j + 2) - 2 * ctrl_now(i * ctrl_num1D + j + 1) + ctrl_now(i * ctrl_num1D + j))
                            * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j - 2) );

            if(j < ctrl_num1D - 3 )
                ret[i+9] += Cj_(j) * traj_order_ * (traj_order_ - 1) * (traj_order_ - 2)
                            * ( ctrl_now(i * ctrl_num1D + j + 3) - 3 * ctrl_now(i * ctrl_num1D + j + 2) + 3 * ctrl_now(i * ctrl_num1D + j + 1) - ctrl_now(i * ctrl_num1D + j))
                            * pow(t_now, j) * pow((1 - t_now), (traj_order_ - j - 3) );
        }
    }

    return ret;
}
/*****  轨迹：获取轨迹且用于发布  ****/
wmr_msgs::PolynomialTrajectory getBezierTraj(){
    wmr_msgs::PolynomialTrajectory traj;
    traj.action = wmr_msgs::PolynomialTrajectory::ACTION_ADD;
    traj.num_segment = seg_num_;

    int order = traj_order_;
    int poly_num1d = order + 1;
    int polyTotalNum = seg_num_ * (order + 1);

    traj.coef_x.resize(polyTotalNum);
    traj.coef_y.resize(polyTotalNum);
    traj.coef_z.resize(polyTotalNum);

    int idx = 0;
    for(int i = 0; i < seg_num_; i++ )
    {
        for(int j =0; j < poly_num1d; j++)
        {
            traj.coef_x[idx] = bezier_coeff_(i,                  j);
            traj.coef_y[idx] = bezier_coeff_(i,     poly_num1d + j);
            traj.coef_z[idx] = bezier_coeff_(i, 2 * poly_num1d + j);
            idx++;
        }
    }

    traj.C.resize(order+1);
    traj.Cv.resize(order);
    traj.Ca.resize(order-1);
    traj.Cj.resize(order-2);
    for (int k = 0; k < order+1; ++k) {
        if (k < order+1)
            traj.C[k] = C_[k];
        if (k < order)
            traj.Cv[k] = Cv_[k];
        if (k < order-1)
            traj.Ca[k] = Ca_[k];
        if (k < order-2)
            traj.Cj[k] = Cj_[k];
    }

    traj.header.frame_id = "/bernstein";
    traj.header.stamp = time_now_;
    start_time_ = traj.header.stamp;

    traj.time.resize(seg_num_);
    traj.order.resize(seg_num_);

    traj.mag_coeff = 1.0;
    for (int idx = 0; idx < seg_num_; ++idx){
        traj.time[idx] = seg_time_(idx);
        traj.order[idx] = traj_order_;
    }

    auto state_begin = getStateFromBezier(bezier_coeff_, 0, 0);

    traj.start_yaw = atan2(state_begin(4), state_begin(3));
    traj.final_yaw = 0.0;

    traj.trajectory_id = traj_id_;
    traj.action = wmr_msgs::PolynomialTrajectory::ACTION_ADD;

    return traj;
}
/*****  轨迹：检测轨迹是否受障碍物阻挡、（经过一段距离后）重新规划轨迹  ****/
bool checkExecTraj()
{
    if(!has_traj_)
        return false;

    Eigen::Vector3d traj_pt;

    visualization_msgs::Marker _check_traj_vis, _stop_traj_vis;

    geometry_msgs::Point pt;
    _stop_traj_vis.header.stamp    = _check_traj_vis.header.stamp    = time_now_;
    _stop_traj_vis.header.frame_id = _check_traj_vis.header.frame_id = "odom";

    _check_traj_vis.ns = "trajectory/check_trajectory";
    _stop_traj_vis.ns  = "trajectory/stop_trajectory";

    _stop_traj_vis.id     = _check_traj_vis.id = 0;
    _stop_traj_vis.type   = _check_traj_vis.type = visualization_msgs::Marker::SPHERE_LIST;
    _stop_traj_vis.action = _check_traj_vis.action = visualization_msgs::Marker::ADD;

    _stop_traj_vis.scale.x = 2.0 * vis_traj_width_;
    _stop_traj_vis.scale.y = 2.0 * vis_traj_width_;
    _stop_traj_vis.scale.z = 2.0 * vis_traj_width_;

    _check_traj_vis.scale.x = 1.5 * vis_traj_width_;
    _check_traj_vis.scale.y = 1.5 * vis_traj_width_;
    _check_traj_vis.scale.z = 1.5 * vis_traj_width_;

    _check_traj_vis.pose.orientation.x = 0.0;
    _check_traj_vis.pose.orientation.y = 0.0;
    _check_traj_vis.pose.orientation.z = 0.0;
    _check_traj_vis.pose.orientation.w = 1.0;

    _stop_traj_vis.pose = _check_traj_vis.pose;

    _stop_traj_vis.color.r = 0.0;
    _stop_traj_vis.color.g = 1.0;
    _stop_traj_vis.color.b = 0.0;
    _stop_traj_vis.color.a = 1.0;

    _check_traj_vis.color.r = 0.0;
    _check_traj_vis.color.g = 0.0;
    _check_traj_vis.color.b = 1.0;
    _check_traj_vis.color.a = 1.0;

    double t_s = max(0.0, (time_now_ - start_time_).toSec());
    int idx;
    for (idx = 0; idx < seg_num_; ++idx)
    {
        if( t_s  > seg_time_(idx) && idx + 1 < seg_num_)
            t_s -= seg_time_(idx);
        else
            break;
    }

    double duration = 0.0;
    double t_ss;
    for(int i = idx; i < seg_num_; i++ )
    {
        t_ss = (i == idx) ? t_s : 0.0;
        for(double t = t_ss; t < seg_time_(i); t += 0.01)
        {
            double t_d = duration + t - t_ss;
            if( t_d > check_horizon_ ) break;
            traj_pt = getPosFromBezier( bezier_coeff_, t/seg_time_(i), i );
            pt.x = traj_pt(0) = seg_time_(i) * traj_pt(0);
            pt.y = traj_pt(1) = seg_time_(i) * traj_pt(1);
            pt.z = traj_pt(2) = seg_time_(i) * traj_pt(2);

            _check_traj_vis.points.push_back(pt);

            if( t_d <= stop_horizon_ )
                _stop_traj_vis.points.push_back(pt);

            if( map_generator_->checkCoordObs(traj_pt))
            {
                ROS_WARN("predicted collision time is %f ahead", t_d);

                if( t_d <= stop_horizon_ )
                {
                    ROS_ERROR("emergency occurs in time is %f ahead", t_d);
                    is_emerg_ = true;
                }

                checkTraj_vis_pub_.publish(_check_traj_vis);
                stopTraj_vis_pub_.publish(_stop_traj_vis);

                return true;
            }
        }
        duration += seg_time_(i) - t_ss;
    }

    checkTraj_vis_pub_.publish(_check_traj_vis);
    stopTraj_vis_pub_.publish(_stop_traj_vis);

    //机器人每前进一段距离重新规划一次轨迹
    if(is_retraj_prog_ && sqrt(pow((start_pt_(0) - traj_start_pt_(0)), 2)
            + pow((start_pt_(1) - traj_start_pt_(1)), 2)) >= 3){
        traj_start_pt_ = start_pt_;
        return true;
    }

    return false;
}
